/* ========================================================================
 * PROJECT: FTIR Blob Tracker
 * ========================================================================
 * This work is based on the original FTIR Blob Tracker developed by
 *   Woonhyuk Baek
 *   Woontack Woo
 *   U-VR Lab, GIST of Gwangju in Korea.
 * 
 *   http://uvr.gist.ac.kr/
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2009 GIST U-VR Lab.
 *
 * This framework is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This framework is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Woonhyuk Baek
 *   <windage@live.com>
 *   GIST U-VR Lab.
 *   Department of Information and Communication
 *   Gwangju Institute of Science and Technology
 *   1, Oryong-dong, Buk-gu, Gwangju
 *   South Korea
 * ========================================================================
 ** @author   Woonhyuk Baek
 * ======================================================================== */

#include <cv.h>

#include "Calibrator.h"
using namespace windage;

void Calibrator::ResetChessboardPointList()
{
	for(unsigned int i=0; i<this->chessboardPointList.size(); i++)
		this->chessboardPointList[i].clear();
	chessboardPointList.clear();
}

bool Calibrator::FindChessboardCorner(std::vector<CvPoint2D32f>* resultPoint, IplImage* grayImage)
{
	int pointCount = this->GetChessboardPointCount();
	CvSize size = cvSize(chessboardWidthCount-1, chessboardHeightCount-1);
	CvPoint2D32f* foundPoints = new CvPoint2D32f[pointCount];

	int result = cvFindChessBoardCornerGuesses(grayImage, NULL, NULL, size, foundPoints, &pointCount);
	if(result)
	{
		cvFindCornerSubPix(grayImage, foundPoints, pointCount, cvSize(5,5), cvSize(-1,-1),
						   cvTermCriteria( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f ));

		for(int i=0; i<pointCount; i++)
			resultPoint->push_back(foundPoints[i]);
	}
	else
	{
		if(foundPoints) delete foundPoints;
		return false;
	}
	if(foundPoints)	delete foundPoints;
	return true;
}

bool Calibrator::FindChessboardCorner(std::vector<windage::Vector2>* resultPoint, IplImage* grayImage)
{
	std::vector<CvPoint2D32f> tempPoint;
	bool result = FindChessboardCorner(&tempPoint, grayImage);

	for(unsigned int i=0; i<tempPoint.size(); i++)
		resultPoint->push_back(windage::Vector2(tempPoint[i].x, tempPoint[i].y));

	return result;
}

bool Calibrator::GetChessboard(std::vector<windage::Vector2>* resultPoint)
{
	for(int y=0; y<chessboardHeightCount-1; y++)
	{
		for(int x=0; x<chessboardWidthCount-1; x++)
		{
			resultPoint->push_back(windage::Vector2((chessboardFieldSize * y), (chessboardFieldSize * x)));
		}
	}

	return true;
}

void Calibrator::DrawChessboardInfo(std::vector<CvPoint2D32f>* resultPoint, IplImage* colorImage)
{
	int pointCount = resultPoint->size();
	double r = 255.0;
	double g = 0.0;
	double b = 0.0;

	cvCircle(colorImage, cvPoint((int)(*resultPoint)[0].x, (int)(*resultPoint)[0].y), 5, CV_RGB(255, 0, 0), CV_FILLED);
	for(int i=1; i<pointCount; i++)
	{
		r = 255.0 - 255.0 * (i/(double)pointCount);
		b = 255.0 * (i/(double)pointCount);

		cvLine(colorImage, cvPoint((int)(*resultPoint)[i-1].x, (int)(*resultPoint)[i-1].y), cvPoint((int)(*resultPoint)[i].x, (int)(*resultPoint)[i].y), CV_RGB(r, g, b), 2);
		cvCircle(colorImage, cvPoint((int)(*resultPoint)[i].x, (int)(*resultPoint)[i].y), 5, CV_RGB(r, g, b), CV_FILLED);
	}
}

bool Calibrator::SolveCalibration()
{
	if(this->calibration == NULL)
		return false;

	int imageCount = chessboardPointList.size();
	int pointCount = this->GetChessboardPointCount();
	int allPointCount = pointCount * imageCount;

	CvMat* pImagePoints = cvCreateMat(allPointCount, 2, CV_64FC1);
    CvMat* objectPoints = cvCreateMat(allPointCount, 3, CV_64FC1);
    CvMat* imagePointCounts = cvCreateMat(imageCount, 1, CV_32SC1);

	for(int y=0; y<imageCount; y++)
	{
		for(int x=0; x<pointCount; x++)
		{
			cvSetReal2D(pImagePoints, x + y*pointCount, 0, chessboardPointList[y][x].x);
			cvSetReal2D(pImagePoints, x + y*pointCount, 1, chessboardPointList[y][x].y);
		}
	}

	for(int y=0; y<chessboardHeightCount-1; y++)
	{
		for(int x=0; x<chessboardWidthCount-1; x++)
		{
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 0, (chessboardFieldSize * y) );
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 1, (chessboardFieldSize * x) );
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 2, 0.0f );
		}
	}

	for(int i=0; i<imageCount; i++)
	{
		for(int x=0; x<pointCount; x++)
		{
			cvSetReal2D(objectPoints, (i*pointCount + x), 0, cvGetReal2D(objectPoints, x, 0) );
			cvSetReal2D(objectPoints, (i*pointCount + x), 1, cvGetReal2D(objectPoints, x, 1) );
			cvSetReal2D(objectPoints, (i*pointCount + x), 2, 0.0f );
		}
	}
	
	for(int i=0; i<imageCount; i++)
	{
		cvSetReal1D(imagePointCounts, i, pointCount);
	}

	// calculate calibration function
    cvCalibrateCamera2(objectPoints, pImagePoints, imagePointCounts,
		this->calibration->GetSize(),
		calibration->GetIntrinsicMatrix(), calibration->GetDistortionCoefficients());

	// release matrix
    if(pImagePoints != NULL) cvReleaseMat(&pImagePoints);
    if(objectPoints != NULL) cvReleaseMat(&objectPoints);
    if(imagePointCounts != NULL) cvReleaseMat(&imagePointCounts);

	return true;
}

bool Calibrator::UpdateExtrinsic(std::vector<CvPoint2D32f>* resultPoint)
{
	if(this->calibration == NULL)
		return false;

	int pointCount = resultPoint->size();

	CvMat* pImagePoints = cvCreateMat(pointCount, 2, CV_64FC1);
	CvMat* objectPoints = cvCreateMat(pointCount, 3, CV_64FC1);

	CvMat* translationVector = cvCreateMat(3, 1, CV_64FC1);
	CvMat* rotationVector = cvCreateMat(3, 1, CV_64FC1);
	CvMat* rotationMatrix = cvCreateMat(3, 3, CV_64FC1);
	CvMat* extrinsicMatrix = cvCreateMat(4, 4, CV_64FC1);
	cvSetIdentity(extrinsicMatrix);

	for(int i=0; i<pointCount; ++i)
	{
		cvSetReal2D(pImagePoints, i, 0, (*resultPoint)[i].x);
		cvSetReal2D(pImagePoints, i, 1, (*resultPoint)[i].y);
	}

	for(int y=0; y<chessboardHeightCount-1; y++)
	{
		for(int x=0; x<chessboardWidthCount-1; x++)
		{
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 0, (chessboardFieldSize * y) );
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 1, (chessboardFieldSize * x) );
			cvSetReal2D(objectPoints, (y * (chessboardWidthCount-1) + x), 2, 0.0f );
		}
	}

	cvFindExtrinsicCameraParams2(objectPoints, pImagePoints,
								this->calibration->GetIntrinsicMatrix(), this->calibration->GetDistortionCoefficients(),\
								rotationVector, translationVector);

	// convert vector to matrix
	for(int i=0; i < 3; i++) 
	{
		cvSetReal2D(extrinsicMatrix, i, 3, cvGetReal1D(translationVector, i));
	} 

	cvRodrigues2(rotationVector, rotationMatrix);
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			cvSetReal2D(extrinsicMatrix, i, j, cvGetReal2D(rotationMatrix, i, j)); 
		}
	}

	// set the extrinsic
	this->calibration->SetExtrinsicMatrix(extrinsicMatrix);

	// release matrix
	if(translationVector != NULL) cvReleaseMat(&translationVector);
	if(rotationVector != NULL) cvReleaseMat(&rotationVector);
	if(rotationMatrix != NULL) cvReleaseMat(&rotationMatrix);
	if(extrinsicMatrix != NULL) cvReleaseMat(&extrinsicMatrix);

	if(pImagePoints != NULL) cvReleaseMat(&pImagePoints);
	if(objectPoints != NULL) cvReleaseMat(&objectPoints);

	return true;
}
